#include <iostream>
#include <vector>
#include <thread>
#include <mutex>
#include <chrono>
#include <cstring>
#include <iomanip>
#include <sstream>
#include <functional>
#include <unordered_map>
#include <map>
#include <array>
#include <queue>
#include <serial/serial.h>

#define TX_debug_ false
#define RX_debug_ false
#define Interval_Ms_ 100  //发送间隔ms
#define RECEIVE_MS_ 100    //接受间隔ms

// 半精度浮点数转换函数
uint16_t floatToHalf(float f) {
    uint32_t x = *reinterpret_cast<uint32_t*>(&f);

    uint32_t sign = (x >> 16) & 0x8000;     // sign bit
    int32_t exp = ((x >> 23) & 0xFF) - 127; // exponent
    uint32_t mant = x & 0x7FFFFF;           // mantissa

    if (exp == 128) { // NaN or Inf
        if (mant == 0) {
            // Inf
            return sign | 0x7C00;
        } else {
            // NaN
            return sign | 0x7C00 | (mant >> 13);
        }
    }

    if (exp > 15) {
        // Overflow - set to Inf
        return sign | 0x7C00;
    } else if (exp > -15) {
        // Normalized number
        exp += 15;
        mant = mant >> 13;
        return sign | (exp << 10) | mant;
    } else if (exp >= -24) {
        // Subnormal number
        mant |= 0x800000;  // add leading 1
        int shift = (-14 - exp);
        mant = mant >> (shift + 13);
        return sign | mant;
    } else {
        // Underflow - set to zero
        return sign;
    }
}

float halfToFloat(uint16_t h) {
    uint32_t x = ((h & 0x8000) << 16) | // 符号位
                 (((h & 0x7c00) + 0x1c000) << 13) | // 指数
                 ((h & 0x03ff) << 13); // 尾数
    return *reinterpret_cast<float*>(&x);
}

// 接收到的天空端数据结构
struct UAVData {
    uint32_t source_address;
    uint8_t uav_id;
    float uav_position_x;
    float uav_position_y;
    float uav_position_z;
    float uav_orientation_x;
    float uav_orientation_y;
    float uav_orientation_z;
    float uav_orientation_w;

    float detect_position_x;
    float detect_position_y;
    float detect_position_z;
    float detect_orientation_x;
    float detect_orientation_y;
    float detect_orientation_z;
    float detect_orientation_w;
    uint8_t battery_percentage;
    uint8_t status_flags;
    uint8_t reserved1;
    uint8_t reserved2;
};

// 控制指令结构
struct ControlCommand {
    float v_x = 0.0f;
    float v_y = 0.0f;
    float v_z = 0.0f;
    float yaw = 0.0f;
    int32_t mode = 0;
    float reserved1;
    float reserved2;
};

class CircularBuffer {
public:
    static const size_t BUFFER_SIZE = 4096;
    
    CircularBuffer() : head_(0), tail_(0) {}
    
    size_t write(const uint8_t* data, size_t size) {
        size_t available = availableToWrite();
        size_t to_write = std::min(size, available);
        
        if (to_write == 0) return 0;
        
        size_t first_chunk = std::min(to_write, BUFFER_SIZE - tail_);
        memcpy(buffer_.data() + tail_, data, first_chunk);
        
        size_t second_chunk = to_write - first_chunk;
        if (second_chunk > 0) {
            memcpy(buffer_.data(), data + first_chunk, second_chunk);
            tail_ = second_chunk;
        } else {
            tail_ += first_chunk;
            if (tail_ == BUFFER_SIZE) tail_ = 0;
        }
        
        return to_write;
    }
    
    size_t read(uint8_t* dest, size_t size) {
        size_t available = availableToRead();
        size_t to_read = std::min(size, available);
        
        if (to_read == 0) return 0;
        
        size_t first_chunk = std::min(to_read, BUFFER_SIZE - head_);
        memcpy(dest, buffer_.data() + head_, first_chunk);
        
        size_t second_chunk = to_read - first_chunk;
        if (second_chunk > 0) {
            memcpy(dest + first_chunk, buffer_.data(), second_chunk);
            head_ = second_chunk;
        } else {
            head_ += first_chunk;
            if (head_ == BUFFER_SIZE) head_ = 0;
        }
        
        return to_read;
    }
    
    size_t availableToRead() const {
        if (tail_ >= head_) return tail_ - head_;
        return BUFFER_SIZE - head_ + tail_;
    }
    
    size_t availableToWrite() const {
        if (head_ > tail_) return head_ - tail_ - 1;
        return BUFFER_SIZE - tail_ + head_ - 1;
    }
    
    void peek(uint8_t* dest, size_t offset, size_t size) const {
        size_t read_pos = (head_ + offset) % BUFFER_SIZE;
        
        if (read_pos + size <= BUFFER_SIZE) {
            memcpy(dest, buffer_.data() + read_pos, size);
        } else {
            size_t first_chunk = BUFFER_SIZE - read_pos;
            memcpy(dest, buffer_.data() + read_pos, first_chunk);
            memcpy(dest + first_chunk, buffer_.data(), size - first_chunk);
        }
    }
    
    void consume(size_t size) {
        head_ = (head_ + size) % BUFFER_SIZE;
    }
    
private:
    std::array<uint8_t, BUFFER_SIZE> buffer_;
    size_t head_;
    size_t tail_;
};

class GroundStation {
public:
    using DataCallback = std::function<void(const UAVData& data)>;
    using LogCallback = std::function<void(const std::string& message)>;

    bool TX_debug = TX_debug_;
    bool RX_debug = RX_debug_;

    static const size_t PACKET_SIZE_NORMAL = 38;
    static const size_t PACKET_SIZE_CONTROL = 19;

    static void defaultLogCallback(const std::string& message) {
        std::cout << "[GroundStation] " << message << std::endl;
    }
    
    GroundStation() : running_(false) {
        setLogCallback(defaultLogCallback);
    }
    
    ~GroundStation() {
        stop();
    }
    
    void setLogCallback(LogCallback callback) {
        std::lock_guard<std::mutex> lock(log_mutex_);
        log_callback_ = callback;
    }
    
    void setDataCallback(DataCallback callback) {
        std::lock_guard<std::mutex> lock(data_mutex_);
        data_callback_ = callback;
    }
    
    void addUAVAddress(uint32_t address) {
        std::lock_guard<std::mutex> lock(config_mutex_);
        uav_addresses_.push_back(address);
        log("Added UAV address: 0x" + toHex(address));
    }
    
    void setControlCommand(uint32_t uav_id, const ControlCommand& cmd) {
        std::lock_guard<std::mutex> lock(control_mutex_);
        control_commands_[uav_id] = cmd;
        log("Set control command for UAV 0x" + toHex(uav_id) + 
            ": v=(" + std::to_string(cmd.v_x) + ", " + 
            std::to_string(cmd.v_y) + ", " + 
            std::to_string(cmd.v_z) + "), yaw=" + 
            std::to_string(cmd.yaw) + ", mode=" + 
            std::to_string(cmd.mode));
    }
    
    bool initSerial(const std::string& port, int baudrate = 115200) {
        std::lock_guard<std::mutex> lock(serial_mutex_);
        
        try {
            serial_port_.setPort(port);
            serial_port_.setBaudrate(baudrate);
            serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
            serial_port_.setTimeout(timeout);
            serial_port_.setBytesize(serial::eightbits);
            serial_port_.setParity(serial::parity_none);
            serial_port_.setStopbits(serial::stopbits_one);
            serial_port_.open();
            
            if (serial_port_.isOpen()) {
                log("Serial port " + port + " opened successfully at " + std::to_string(baudrate) + " baud");
                return true;
            } else {
                log("Failed to open serial port " + port);
                return false;
            }
        } catch (const std::exception& e) {
            log(std::string("Serial port initialization failed: ") + e.what());
            return false;
        }
    }
    
    bool start() {
        if (running_) {
            log("GroundStation is already running");
            return false;
        }
        
        if (!serial_port_.isOpen()) {
            log("Serial port not open, cannot start");
            return false;
        }
        
        if (uav_addresses_.empty()) {
            log("No UAV addresses configured, cannot start");
            return false;
        }
        
        running_ = true;
        send_thread_ = std::thread(&GroundStation::sendThread, this);
        receive_thread_ = std::thread(&GroundStation::receiveThread, this);
        
        log("GroundStation started");
        return true;
    }
    
    void stop() {
        if (!running_) return;
        
        running_ = false;
        
        if (send_thread_.joinable()) send_thread_.join();
        if (receive_thread_.joinable()) receive_thread_.join();
        
        if (serial_port_.isOpen()) {
            serial_port_.close();
        }
        
        log("GroundStation stopped");
    }
    
    bool isRunning() const {
        return running_;
    }

private:
    struct FrameRateStats {
        uint32_t frame_count = 0;
        std::chrono::steady_clock::time_point last_report_time;
        std::chrono::steady_clock::time_point last_frame_time;
    };

    void log(const std::string& message) {
        std::lock_guard<std::mutex> lock(log_mutex_);
        if (log_callback_) {
            log_callback_(message);
        }
    }
    
    std::string toHex(uint32_t value, int width = 6) {
        std::stringstream ss;
        ss << std::hex << std::setw(width) << std::setfill('0') << value;
        return ss.str();
    }
    
    bool reconnectSerial() {
        log("Attempting to reconnect to serial port...");
        try {
            if (serial_port_.isOpen()) {
                serial_port_.close();
            }
            serial_port_.open();
            
            if (serial_port_.isOpen()) {
                log("Serial port reconnected successfully");
                return true;
            } else {
                log("Failed to reconnect to serial port");
                return false;
            }
        } catch (const std::exception& e) {
            log(std::string("Serial reconnection failed: ") + e.what());
            return false;
        }
    }
    
    void updateFrameRateStats(uint32_t source_address) {
        std::lock_guard<std::mutex> lock(stats_mutex_);
        auto now = std::chrono::steady_clock::now();
        
        FrameRateStats& stats = frame_rate_stats_[source_address];
        
        if (stats.frame_count == 0) {
            stats.last_report_time = now;
        }
        
        stats.frame_count++;
        stats.last_frame_time = now;
        
        if (std::chrono::duration_cast<std::chrono::seconds>(now - stats.last_report_time).count() >= 1) {
            double fps = stats.frame_count / 
                        std::chrono::duration_cast<std::chrono::duration<double>>(
                            now - stats.last_report_time).count();
            
            std::stringstream ss;
            ss << "Frame rate for 0x" << std::hex << source_address << std::dec 
               << ": " << std::fixed << std::setprecision(2) << fps << " FPS"
               << " (Packets: " << stats.frame_count << ")";
            log(ss.str());
            
            stats.frame_count = 0;
            stats.last_report_time = now;
        }
    }
    
    void cleanUpStaleStats() {
        std::lock_guard<std::mutex> lock(stats_mutex_);
        auto now = std::chrono::steady_clock::now();
        
        auto it = frame_rate_stats_.begin();
        while (it != frame_rate_stats_.end()) {
            if (std::chrono::duration_cast<std::chrono::seconds>(
                    now - it->second.last_frame_time).count() > 10) {
                
                std::stringstream ss;
                ss << "Removing stale stats for 0x" << std::hex << it->first << std::dec;
                log(ss.str());
                
                it = frame_rate_stats_.erase(it);
            } else {
                ++it;
            }
        }
    }
    
    void sendThread() {
        const int interval_ms = Interval_Ms_;
        
        while (running_) {
            auto start_time = std::chrono::steady_clock::now();
            
            for (uint32_t addr : uav_addresses_) {
                sendControlCommand(addr);
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
            }
            
            auto elapsed = std::chrono::steady_clock::now() - start_time;
            auto sleep_time = std::chrono::milliseconds(interval_ms) - elapsed;
            if (sleep_time.count() > 0) {
                std::this_thread::sleep_for(sleep_time);
            }
        }
    }
    
    void sendControlCommand(uint32_t target_addr) {
        ControlCommand cmd;
        {
            std::lock_guard<std::mutex> lock(control_mutex_);
            auto it = control_commands_.find(target_addr);
            if (it != control_commands_.end()) {
                cmd = it->second;
            }
        }
        
        std::vector<uint8_t> packet = serializeControlCommand(target_addr, cmd);
        
        {
            std::lock_guard<std::mutex> lock(serial_mutex_);
            try {
                if (!serial_port_.isOpen()) {
                    if (!reconnectSerial()) {
                        return;
                    }
                }
                
                size_t bytes_written = serial_port_.write(packet);
                if (bytes_written != packet.size()) {
                    log("Incomplete data sent. Expected: " + std::to_string(packet.size()) + 
                        ", Actual: " + std::to_string(bytes_written));
                }
            } catch (const std::exception& e) {
                log(std::string("Serial write error: ") + e.what());
            }
        }
    }
    
    std::vector<uint8_t> serializeControlCommand(uint32_t target_addr, const ControlCommand& cmd) {
        std::vector<uint8_t> packet;
        
        packet.push_back((target_addr >> 16) & 0xFF);
        packet.push_back((target_addr >> 8) & 0xFF);
        packet.push_back(target_addr & 0xFF);

        packet.push_back(0xAA);
        packet.push_back(0x55);
        packet.push_back(0x04);
        
        appendHalf(packet, cmd.v_x);
        appendHalf(packet, cmd.v_y);
        appendHalf(packet, cmd.v_z);
        appendHalf(packet, cmd.yaw);
        appendHalf(packet, cmd.reserved1);
        appendHalf(packet, cmd.reserved2);
        
        packet.push_back(static_cast<uint8_t>(cmd.mode));
        
        uint8_t checksum = 0;
        for (size_t i = 3; i < packet.size(); ++i) {
            checksum ^= packet[i];
        }
        packet.push_back(checksum);
        
        packet.push_back(0x55);
        packet.push_back(0xAA);
        
        if (TX_debug) {
            std::stringstream ss;
            ss << "Sending control command to 0x" << std::hex << target_addr << std::dec 
               << ": v=(" << cmd.v_x << ", " << cmd.v_y << ", " << cmd.v_z 
               << "), yaw=" << cmd.yaw << ", mode=" << cmd.mode;
            log(ss.str());
        }

        return packet;
    }
    
    void appendHalf(std::vector<uint8_t>& packet, float value) {
        uint16_t half_value = floatToHalf(value);
        packet.push_back(half_value & 0xFF);
        packet.push_back((half_value >> 8) & 0xFF);
    }

    void receiveThread() {
        CircularBuffer ring_buffer;
        const int RECONNECT_INTERVAL_MS = 2000;
        const int CLEANUP_INTERVAL_MS = 5000;
        auto last_reconnect_time = std::chrono::steady_clock::now();
        auto last_cleanup = std::chrono::steady_clock::now();
        
        while (running_) {
            bool port_open = false;
            {
                std::lock_guard<std::mutex> lock(serial_mutex_);
                port_open = serial_port_.isOpen();
            }
            
            if (!port_open) {
                auto now = std::chrono::steady_clock::now();
                if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_reconnect_time).count() > RECONNECT_INTERVAL_MS) {
                    if (reconnectSerial()) {
                        log("Serial port reconnected in receive thread");
                    } else {
                        log("Reconnect failed in receive thread");
                    }
                    last_reconnect_time = now;
                }
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
                continue;
            }
            
            try {
                std::lock_guard<std::mutex> lock(serial_mutex_);
                size_t available = serial_port_.available();
                if (available > 0) {
                    size_t to_read = std::min(available, ring_buffer.availableToWrite());
                    if (to_read > 0) {
                        std::vector<uint8_t> temp_buffer(to_read);
                        size_t bytes_read = serial_port_.read(temp_buffer.data(), to_read);
                        
                        if (bytes_read > 0) {
                            ring_buffer.write(temp_buffer.data(), bytes_read);
                            
                            if (RX_debug) {
                                std::stringstream ss;
                                ss << "Received " << bytes_read << " bytes. ";
                                log(ss.str());
                            }
                        }
                    }
                }
            } catch (const std::exception& e) {
                log(std::string("Serial read error: ") + e.what());
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
                continue;
            }
            
            processAllPacketsInBuffer(ring_buffer);
            
            auto now = std::chrono::steady_clock::now();
            if (std::chrono::duration_cast<std::chrono::milliseconds>(
                    now - last_cleanup).count() > CLEANUP_INTERVAL_MS) {
                cleanUpStaleStats();
                last_cleanup = now;
            }
            
            std::this_thread::sleep_for(std::chrono::milliseconds(RECEIVE_MS_));
        }
    }

    void processAllPacketsInBuffer(CircularBuffer& ring_buffer) {
        const size_t MIN_PACKET_SIZE = 6;
        size_t processed_packets = 0;
        const size_t MAX_PROCESS_PER_CYCLE = 20;
        
        while (ring_buffer.availableToRead() >= MIN_PACKET_SIZE && 
               processed_packets < MAX_PROCESS_PER_CYCLE) {
            
            bool found_header = false;
            size_t header_pos = 0;
            
            for (size_t i = 0; i <= ring_buffer.availableToRead() - 1; i++) {
                uint8_t byte1, byte2;
                ring_buffer.peek(&byte1, i, 1);
                ring_buffer.peek(&byte2, i + 1, 1);
                
                if (byte1 == 0xAA && byte2 == 0x55) {
                    found_header = true;
                    header_pos = i;
                    break;
                }
            }
            
            if (!found_header) {
                ring_buffer.consume(ring_buffer.availableToRead());
                return;
            }
            
            if (header_pos > 0) {
                ring_buffer.consume(header_pos);
            }
            
            if (ring_buffer.availableToRead() < 3) {
                return;
            }
            
            uint8_t packet_id;
            ring_buffer.peek(&packet_id, 2, 1);
            
            size_t expected_packet_size = 0;
            if (packet_id >= 0x01 && packet_id <= 0x03) {
                expected_packet_size = PACKET_SIZE_NORMAL;
            } else if (packet_id == 0x04) {
                expected_packet_size = PACKET_SIZE_CONTROL;
            } else {
                ring_buffer.consume(1);
                continue;
            }
            
            if (ring_buffer.availableToRead() < expected_packet_size) {
                return;
            }
            
            std::vector<uint8_t> packet(expected_packet_size);
            ring_buffer.read(packet.data(), expected_packet_size);
            
            size_t footer_pos = expected_packet_size - 2;
            if (packet[footer_pos] != 0x55 || packet[footer_pos + 1] != 0xAA) {
                if (RX_debug) {
                    std::stringstream ss;
                    ss << "Invalid packet tail for ID 0x" << std::hex << static_cast<int>(packet_id)
                       << ": expected 0x55 0xAA, got 0x" << std::hex 
                       << static_cast<int>(packet[footer_pos]) << " 0x"
                       << static_cast<int>(packet[footer_pos + 1]);
                    log(ss.str());
                }
                continue;
            }
            
            uint8_t calc_checksum = 0;
            for (size_t j = 0; j < expected_packet_size - 3; ++j) {
                calc_checksum ^= packet[j];
            }
            
            uint8_t recv_checksum = packet[expected_packet_size - 3];
            if (calc_checksum != recv_checksum) {
                if (RX_debug) {
                    std::stringstream ss;
                    ss << "Checksum error for ID 0x" << std::hex << static_cast<int>(packet_id)
                       << ": expected 0x" << static_cast<int>(calc_checksum)
                       << ", got 0x" << static_cast<int>(recv_checksum);
                    log(ss.str());
                }
                continue;
            }
            
            bool parse_success = false;
            if (packet_id == 0x04) {
                // 地面站不应接收控制包
            } else { 
                parse_success = parseNormalDataPacket(packet, 0);
            }
            
            if (parse_success) {
                processed_packets++;
                if (RX_debug) {
                    log("Successfully parsed data for ID 0x" + toHex(packet_id, 2));
                }
            }
        }
        
        if (RX_debug && processed_packets > 0) {
            log("Processed " + std::to_string(processed_packets) + " packets");
        }
    }

    bool parseNormalDataPacket(const std::vector<uint8_t>& buffer, size_t start_pos) {
        if (start_pos + PACKET_SIZE_NORMAL > buffer.size()) return false;
    
        UAVData data;
        
        data.source_address = (static_cast<uint32_t>(0x00) << 16) |
                            (static_cast<uint32_t>(buffer[start_pos + 2]) << 8) |
                            static_cast<uint32_t>(0x17);
        
        data.uav_id = buffer[start_pos + 2];
        
        data.uav_position_x = parseHalf(buffer, start_pos + 3);
        data.uav_position_y = parseHalf(buffer, start_pos + 5);
        data.uav_position_z = parseHalf(buffer, start_pos + 7);
        
        data.uav_orientation_x = parseHalf(buffer, start_pos + 9);
        data.uav_orientation_y = parseHalf(buffer, start_pos + 11);
        data.uav_orientation_z = parseHalf(buffer, start_pos + 13);
        data.uav_orientation_w = parseHalf(buffer, start_pos + 15);
        
        data.detect_position_x = parseHalf(buffer, start_pos + 17);
        data.detect_position_y = parseHalf(buffer, start_pos + 19);
        data.detect_position_z = parseHalf(buffer, start_pos + 21);
        
        data.detect_orientation_x = parseHalf(buffer, start_pos + 23);
        data.detect_orientation_y = parseHalf(buffer, start_pos + 25);
        data.detect_orientation_z = parseHalf(buffer, start_pos + 27);
        data.detect_orientation_w = parseHalf(buffer, start_pos + 29);
        
        data.battery_percentage = buffer[start_pos + 31];
        data.status_flags = buffer[start_pos + 32];
        data.reserved1 = buffer[start_pos + 33];
        data.reserved2 = buffer[start_pos + 34];
        
        if (RX_debug) {
            std::stringstream ss;
            ss << "UAV Data from 0x" << std::hex << data.source_address << std::dec
            << " (ID: " << static_cast<int>(data.uav_id) << ")\n"
            << "Position: (" << data.uav_position_x << ", " << data.uav_position_y << ", " << data.uav_position_z << ")\n"
            << "Detect Position: (" << data.detect_position_x << ", " << data.detect_position_y << ", " << data.detect_position_z << ")\n"
            << "Battery: " << static_cast<int>(data.battery_percentage) << "%\n"
            << "Status: 0x" << std::hex << static_cast<int>(data.status_flags);
            log(ss.str());
        }
        
        {
            std::lock_guard<std::mutex> lock(data_mutex_);
            if (data_callback_) {
                data_callback_(data);
            }
        }
        
        updateFrameRateStats(data.source_address);
        
        return true;
    }

    float parseHalf(const std::vector<uint8_t>& buffer, size_t start_pos) {
        uint16_t half_value = static_cast<uint16_t>(buffer[start_pos]) | 
                             (static_cast<uint16_t>(buffer[start_pos + 1]) << 8);
        return halfToFloat(half_value);
    }

private:
    serial::Serial serial_port_;
    bool running_;
    std::vector<uint32_t> uav_addresses_;
    std::unordered_map<uint32_t, ControlCommand> control_commands_;
    std::map<uint32_t, FrameRateStats> frame_rate_stats_;
    DataCallback data_callback_;
    LogCallback log_callback_;
    std::thread send_thread_;
    std::thread receive_thread_;
    std::mutex serial_mutex_;
    std::mutex config_mutex_;
    std::mutex control_mutex_;
    std::mutex data_mutex_;
    std::mutex log_mutex_;
    std::mutex stats_mutex_;
};

int main() {
    GroundStation groundStation;

    if(RX_debug_){
        groundStation.setLogCallback([](const std::string& msg) {
            std::cout << "[LOG] " << msg << std::endl;
        });
        groundStation.setDataCallback([](const UAVData& data) {
            std::cout << "Received data from UAV 0x" << std::hex << data.source_address << std::dec << " (ID: " 
                    << static_cast<int>(data.uav_id) << ")\n";
            std::cout << "Position: (" << data.uav_position_x << ", " 
                    << data.uav_position_y << ", " << data.uav_position_z << ")\n";
            std::cout << "Detect Position: (" << data.detect_position_x << ", " 
                    << data.detect_position_y << ", " << data.detect_position_z << ")\n";
            std::cout << "Battery: " << static_cast<int>(data.battery_percentage) << "%\n";
            std::cout << "Status: 0x" << std::hex << static_cast<int>(data.status_flags) << std::dec << "\n";
            std::cout << "---------------------------------\n";
        });
    }
    
    groundStation.addUAVAddress(0x000117);
    groundStation.addUAVAddress(0x000217);
    groundStation.addUAVAddress(0x000317);
    
    ControlCommand cmd1;
    cmd1.v_x = 0.5f;
    cmd1.v_y = 0.2f;
    cmd1.v_z = 0.1f;
    cmd1.yaw = 0.3f;
    cmd1.reserved1 = 0.0f;
    cmd1.reserved2 = 0.0f;
    cmd1.mode = 1;
    groundStation.setControlCommand(0x000117, cmd1);
    
    ControlCommand cmd2;
    cmd2.v_x = -0.3f;
    cmd2.v_y = 0.4f;
    cmd2.v_z = -0.2f;
    cmd2.yaw = -0.1f;
    cmd2.reserved1 = 0.0f;
    cmd2.reserved2 = 0.0f;
    cmd2.mode = 2;
    groundStation.setControlCommand(0x000217, cmd2);
    
    ControlCommand cmd3;
    cmd3.v_x = 0.1f;
    cmd3.v_y = -0.2f;
    cmd3.v_z = 0.3f;
    cmd3.yaw = 0.2f;
    cmd3.reserved1 = 0.0f;
    cmd3.reserved2 = 0.0f;
    cmd3.mode = 3;
    groundStation.setControlCommand(0x000317, cmd3);
    
    if (!groundStation.initSerial("/dev/ttyUSB0", 115200)) {
        std::cerr << "Failed to initialize serial port" << std::endl;
        return 1;
    }
    
    if (!groundStation.start()) {
        std::cerr << "Failed to start ground station" << std::endl;
        return 1;
    }
    
    std::cout << "Ground station running. Press Enter to exit..." << std::endl;
    std::cin.ignore();
    
    groundStation.stop();
    
    return 0;
}